package com.gitee.wsl.mathematics.complex.quaternion.ext

import com.gitee.wsl.mathematics.complex.quaternion.Quaternion
import com.gitee.wsl.mathematics.number.compareTo
import com.gitee.wsl.mathematics.vector.vec3.MutableVector3
import kotlin.math.PI


val <T:Number> Quaternion<T, *>.euler : MutableVector3<T, *>
    get(){
    val sinrcosp = 2 * (w * x + y * z)
    val cosrsinp = 1 - 2 * (x * x + y * y)
    val sinp = 2 * (w * y - z * x)
    val sinycosp = 2 * (w * z + x * y)
    val cosysinp = 1 - 2 * (y * y + z * z)

    val y = if (abs(sinp) >= one) PI * 0.5 * sign(sinp) else asin(sinp)
    return createVec3(
        atan2(sinrcosp, cosrsinp),
        y,
        atan2(sinycosp, cosysinp)
    )

}